close all;

%%
% Plotting constants
t_max = 1000;

% Animal-specific constants
S_b = 0.5 / sqrt(0.3); % Basic Sinuosity: sigma / sqrt(P)
k = 0.9; % klinokinetic factor (0 < k = a*c < 1).

%%
% Animated lines
h = animatedline('Color', 'b', 'linewidth', 1);
h2 = animatedline('Color', 'r', 'linewidth', 1, 'MaximumNumPoints', 10);

%%
% Initial step

% set the x, y as (0, 0)
x_pos = 0;
y_pos = 0;

% Decide a random orientation theta
theta = rand(1) * pi;

% set the distance
D = 0;
dD = 0;

%%
% For each step,

for i = 2:t_max
    % Decide path length P
    % Here, as a placeholder, we fix P as 2
    P = 0.3;
    
    % Decide sigma_b and sigma(i) from previous distance
    sigma_b = S_b * sqrt(P);
    sigma = sigma_b * (1 + k * dD / P);
    
    % Decide alpha, following the normal distribution
    alpha = normrnd(0, sigma);
    
    % Decide theta
    prev_theta = theta;
    theta = prev_theta + alpha;
    
    % Decide the (x, y) position
    prev_x_pos = x_pos;
    prev_y_pos = y_pos;
    x_pos = prev_x_pos + P * cos(theta);
    y_pos = prev_y_pos + P * sin(theta);
    
    % Update the distance
    prev_D = D;
    D = sqrt(x_pos^2 + y_pos^2);
    dD = D - prev_D;
    
    x_ = [prev_x_pos, x_pos];
    y_ = [prev_y_pos, y_pos];
    addpoints(h, x_, y_);
    addpoints(h2, x_, y_);
    
end

drawnow;